import threading, Queue, random, thread,time
from math import *
import agent_logger
# This holds all of the agents, and allows for easy human control/selection of them during simulation.  It manages keyboard bindings for control of the current agent.  The agent list may have zero or more agents in it.
# TODO Decide if this should continue to use the real control interface, or if it should be abstracted out to another class (probably a better idea)
class agent_controller(object):
	def __init__(self,agents={}):
		self.agents=agents
		self.cur_agent=0
		self.manual_world_velocity=1
	def next_agent(self):
		"""Moves to the next valid agent"""
		if len(self.agents) > 0:
			temp_key_list=self.agents.keys()
			new_agent_num=(temp_key_list.index(self.cur_agent)+1) % len(self.agents) # This will give us the index into the temp_key_list of the next agent
			self.cur_agent=temp_key_list[new_agent_num]
			# TODO We will need to notify the agents involved about the change
		
	def new_agent(self,agent_num,agent):
		# TODO Check that the agent doesn't already exist
		self.agents[agent_num]=agent
	def remove_agent(self, agent_num): # removes current agent from agent list
		# TODO We will need to setup a destructor or a function to call when the agent is removed
		remove_agent_num=agent_num
		self.next_agent()
		del self.agents[remove_agent_num]
	# TODO This needs to be corrected so that if an event comes in to delete 
	#an arbitrary agent (from the controller) that it cleans it up elsewhere also
	#def remove_agent_event(self, agent_num): # removes current agent from agent list
	#	self.remove_agent(self.cur_agent)
	
	def seek(self, target_location):
	#TODO Modify this function to work only with a given agent; This was only used for testing
		target=(target_location[0], target_location[1])
		for i in range(len(self.agents)):
			if i != self.cur_agent:
				self.agents[i].actions['seek'](i, target)
				
	# Agent Control Settings (to be interfaced with the external controls)
	# All amounts will be 0 or infinite to allow the agent to move at maximum physical speed
	def move_forward(self):
		self.agents[self.cur_agent].actions['move_rel_agent'](self.cur_agent,[0,float(1e10000)])
		self.update_sensors()
	def move_backward(self):
		self.agents[self.cur_agent].actions['move_rel_agent'](self.cur_agent,[0.0,float(-1e10000)])
		self.update_sensors()
	def strafe_left(self):
		self.agents[self.cur_agent].actions['move_rel_agent'](self.cur_agent,[float(-1e10000),0.0])
		self.update_sensors()
	def strafe_right(self):
		self.agents[self.cur_agent].actions['move_rel_agent'](self.cur_agent,[float(1e10000),0.0])
		self.update_sensors()
	def move_up(self):
		self.agents[self.cur_agent].actions['move_rel_world'](self.cur_agent,[0.0,-self.manual_world_velocity])
		self.update_sensors()
	def move_down(self):
		self.agents[self.cur_agent].actions['move_rel_world'](self.cur_agent,[0.0,self.manual_world_velocity])
		self.update_sensors()
	def move_left(self):
		self.agents[self.cur_agent].actions['move_rel_world'](self.cur_agent,[-self.manual_world_velocity,0.0])
		self.update_sensors()
	def move_right(self):
		self.agents[self.cur_agent].actions['move_rel_world'](self.cur_agent,[self.manual_world_velocity,0])
		self.update_sensors()
	def rotate_ccw(self):
		self.agents[self.cur_agent].actions['turn_rel'](self.cur_agent,float(-1e10000))
		self.update_sensors()
	def rotate_cw(self):
		self.agents[self.cur_agent].actions['turn_rel'](self.cur_agent,float(1e10000))
		self.update_sensors()
	def dec_man_world_vel(self):
		self.manual_world_velocity=max(self.manual_world_velocity-.1,.1)
		print(self.manual_world_velocity)
	def inc_man_world_vel(self):
		self.manual_world_velocity=max(self.manual_world_velocity+.1,.1)
		print(self.manual_world_velocity)
		

	# This is only relevant for the agent_logger
	def save_log(self):
		if isinstance(self.agents[self.cur_agent],agent_logger.agent_logger):
			self.agents[self.cur_agent].save()
	def toggle_logging(self):
		if isinstance(self.agents[self.cur_agent],agent_logger.agent_logger):
			self.agents[self.cur_agent].toggle_logging()
	def update_sensors(self, agent_num=None):
		if not agent_num:
			agent_num = self.cur_agent
		for sense in self.agents[agent_num].senses:
			print(str(sense)+': '+str(self.agents[agent_num].senses[sense]()))
